#pragma once

#include <rclcpp/rclcpp.hpp>
#include <string>
#include <mutex>
#include <cstdint>
#include <termios.h>  // 添加POSIX串口头文件
#include <fcntl.h>    // 文件控制定义

// 帧头尾定义
constexpr uint8_t FRAME_HEADER = 0x55;
constexpr uint8_t FRAME_TAIL   = 0xBB;

// 命令码定义
constexpr uint8_t TEST_CMD = 0x02;      // 测试命令：小车速度控制
constexpr uint8_t CMD_IMU_DATA = 0xA0;  // IMU数据命令码
constexpr uint8_t CMD_SPEED_DATA = 0xA1; // 速度数据命令码

// 速度数据结构（用于里程计计算）
struct SpeedData {
    int16_t vx; // X轴速度 (mm/s)
    int16_t vy; // Y轴速度 (mm/s)
    int16_t vz; // Z轴角速度 (rad/s)
};

// IMU数据结构
struct ImuData {
    uint32_t timestamp; // 时间戳 (ms)
    float accel_x;      // X加速度 (m/s²)
    float accel_y;      // Y加速度 (m/s²)
    float accel_z;      // Z加速度 (m/s²)
    float gyro_x;       // X角速度 (rad/s)
    float gyro_y;       // Y角速度 (rad/s)
    float gyro_z;       // Z角速度 (rad/s)
};

/**
 * @class SerialBridge
 * @brief ROS2节点，用于处理串口通信，解析IMU和速度数据
 * 
 * 该类负责：
 * 1. 初始化串口设备（使用POSIX标准API）
 * 2. 发送速度控制命令
 * 3. 接收并解析IMU和速度数据帧
 * 4. 提供线程安全的接口获取最新数据
 */
class SerialBridge : public rclcpp::Node {
public:
    /**
     * @brief 构造函数
     * @param port 串口设备路径（如 "/dev/ttyUSB0"）
     * @param baudrate 波特率（如 115200）
     */
    SerialBridge(const std::string& port, int baudrate);
    
    /**
     * @brief 析构函数，关闭串口设备
     */
    ~SerialBridge();
    
    /**
     * @brief 发送速度控制命令到底层硬件
     * @param vx X轴速度 (mm/s)
     * @param vy Y轴速度 (mm/s)
     * @param vz Z轴角速度 (rad/s)
     */
    void setSpeedCommand(int16_t vx, int16_t vy, int16_t vz);
    
    /**
     * @brief 获取最新的速度数据（线程安全）
     * @return 当前速度数据结构体
     */
    SpeedData getSpeedData() const {
        std::lock_guard<std::mutex> lock(data_mutex_);
        return speed_data_;
    }
    
    /**
     * @brief 获取最新的IMU数据（线程安全）
     * @return 当前IMU数据结构体
     */
    ImuData getImuData() const {
        std::lock_guard<std::mutex> lock(data_mutex_);
        return imu_data_;
    }

private:
    // 内部数据处理方法
    void parseData();                   // 定时解析串口数据
    void sendCommand(const uint8_t* data, size_t length); // 发送原始命令
    void processFrame();                // 处理完整的数据帧
    
    int serial_fd_;          // 串口文件描述符（POSIX文件描述符）
    std::string port_;       // 串口设备路径
    int baudrate_;           // 波特率
    
    // 共享数据保护
    mutable std::mutex data_mutex_; // 数据互斥锁
    SpeedData speed_data_;   // 最新速度数据
    ImuData imu_data_;       // 最新IMU数据
    
    // 定时解析器（每10ms触发一次）
    rclcpp::TimerBase::SharedPtr parse_timer_;
    
    // 帧解析状态机相关变量
    enum class FrameState {
        IDLE,       // 空闲状态，等待帧头
        HEADER1,    // 等待帧头1 (0x55)
        COMMAND,    // 等待命令码
        LENGTH,     // 等待数据长度
        DATA,       // 接收数据载荷
        CHECKSUM,   // 等待校验和
        TAIL        // 等待帧尾 (0xBB)
    } frame_state_;
    
    uint8_t command_;       // 当前帧的命令码
    uint8_t length_;        // 当前帧的数据长度
    uint8_t rx_buffer_[256]; // 接收数据缓冲区
    uint8_t data_index_;     // 当前数据位置索引
    uint8_t checksum_;       // 当前帧的校验和累加值
};